/*
 * Miccsnookersim, a snooker simulation with AI
 * Copyright (C) 2009 Enno Ruijters, Frans van den Heuvel, Yannick Thimister,
 * Ali B. Ozmen, Esther Verhoef, Achim Leydecker
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.

 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

import java.awt.geom.*;

public class Physics
{
	private static final double FRICTION_V_AFTER_COLLISION = 0.8;
	private static final double CUE_MASS_RATIO = 0.2;
	private static final double BALL_BALL_COEFF_FRICTION = 0.05;
	private static final double BALL_EDGE_COEFF_FRICTION = 0.8;
	private static final double BALL_EDGE_COEFF_LINEAR_FRICTION = 0.95;

	/**
	 * Corrects the position of two balls so that they do not intersect.
	 * The balls are moved directly away from each other. The ratio of
	 * the distances traveled of the balls is equal to the ratio of the
	 * velocities of the balls.
	 */
	public static void correctPositionBalls(Ball a, Ball b)
	{
		double minDistance = a.getRadius() + b.getRadius() + 0.05;
		double distance = Vector.subtract(a.getPosition(),
		                                  b.getPosition()).magnitude();
		double fractionA = a.getVelocity().magnitude() /
		                   (a.getVelocity().magnitude()
		                    + b.getVelocity().magnitude());
		double fractionB = b.getVelocity().magnitude() /
		                   (a.getVelocity().magnitude()
		                    + b.getVelocity().magnitude());
		Vector normal = Vector.subtract(a.getPosition(),
		                                b.getPosition()).normalize_();
		if (fractionA != fractionA || fractionB != fractionB)
			fractionA = fractionB = 1; /* NaN is not useful */
		double distanceA = (minDistance - distance) * fractionA;
		double distanceB = (minDistance - distance) * fractionB;
		Vector movementA = normal.clone();
		movementA.scaleTo_(distanceA);
		Vector movementB = normal;
		movementB.scaleTo_(distanceB);
		a.getPosition().add_(movementA);
		b.getPosition().sub_(movementB);
	}

	/**
	 * Corrects a ball so that it no longer intersects an edge.
	 * The ball moves exactly normal to the edge and leaves 0.1 mm of space
	 * between it and the edge.
	 */
	private static void correctBallPosition(Ball a, Line2D l)
	{
		while (areColliding(a, l))
			a.setPosition(a.getPosition().add_(a.getVelocity().clone().scale_(0.001)));
	}

	/**
	 * Calculates and sets the new velocity vectors for both balls after
	 * collision.
	 */
	public static void ballCollision(Ball a, Ball b)
	{
		Vector Vrelative = Vector.subtract(a.getVelocity(),
		                                   b.getVelocity());
		Vector normal = Vector.subtract(a.getPosition(),
		                                b.getPosition()).normalize_();
		double deltaV = Vector.dotProduct(Vrelative, normal);
		Vector impulse = normal.scale_(deltaV);
		impulse.scale_(FRICTION_V_AFTER_COLLISION);
		a.getVelocity().sub_(impulse);
		b.getVelocity().add_(impulse);
		double angle = Vector.angle(normal, new Vector(0, 1, 0));
		a.getAngularVelocity().rotate_(-angle);
		b.getAngularVelocity().rotate_(-angle);
		double deltaZ = a.getAngularVelocity().getElements()[2]
		                - b.getAngularVelocity().getElements()[2];
		double deltaX = a.getAngularVelocity().getElements()[0]
		                - b.getAngularVelocity().getElements()[0];
		a.getAngularVelocity().sub_(
		        new Vector(deltaX, 0, deltaZ).scale_(BALL_BALL_COEFF_FRICTION));
		b.getAngularVelocity().add_(
		        new Vector(deltaX, 0, deltaZ).scale_(BALL_BALL_COEFF_FRICTION));
		a.getAngularVelocity().rotate_(angle);
		b.getAngularVelocity().rotate_(angle);
		correctPositionBalls(a, b);
	}

	public static boolean areColliding(Ball ballA, Object objectB)
	{
		if (objectB instanceof Ball) {
			Ball ballB = (Ball) objectB;
			final double minDistance = ballA.getRadius()
			                           + ballB.getRadius();
			/* the minimum distance at which they are not yet
			 * colliding
			 */
			Vector relativeVelocity = Vector.subtract(
			                                   ballB.getVelocity(),
			                                   ballA.getVelocity());
			Vector distanceVector = Vector.subtract(
			                                   ballB.getPosition(),
			                                   ballA.getPosition());
			if (Vector.dotProduct(relativeVelocity,
			                         distanceVector) > 0)
				return false;
			return ballA.distanceFrom(ballB) < minDistance;
		} else if (objectB instanceof Line2D) {
			Line2D lineB = (Line2D) objectB;
			Vector ballPosition = ballA.getPosition();
			double[] coords = ballPosition.getElements();
			double distance = lineB.ptSegDist(coords[0], coords[1]);
			return distance < ballA.getRadius();
		} else {
			throw new ClassCastException();
		}
	}

	public static void ballEdgeCollision(Ball ball, Line2D edge)
	{
		final double lineDeltaX = edge.getX1() - edge.getX2();
		final double lineDeltaY = edge.getY1() - edge.getY2();
		Vector lineVector = new Vector(lineDeltaX, lineDeltaY, 0);
		Vector straightVector= new Vector(1, 0, 0);
		double edgeAngle = Vector.angle(lineVector, straightVector);
		Vector ballVelocity = ball.getVelocity();
		ballVelocity = ballVelocity.rotate_(-edgeAngle).mirrorY_();
		ballVelocity.mul_(new Vector(BALL_EDGE_COEFF_LINEAR_FRICTION,
		                             BALL_EDGE_COEFF_LINEAR_FRICTION,
		                             1));
		ballVelocity = ballVelocity.rotate_(edgeAngle);
		ball.setVelocity(ballVelocity);
		correctBallPosition(ball, edge);
		/* Linear velocity and positions of ball now correctly set */
		Vector AVrelative = ball.getAngularVelocity().clone();
		AVrelative = AVrelative.scale_(BALL_EDGE_COEFF_FRICTION);
		ball.getAngularVelocity().rotate_(edgeAngle);
		double deltaZ = ball.getAngularVelocity().getElements()[2];
		double deltaX = ball.getAngularVelocity().getElements()[0];
		ball.getAngularVelocity().sub_(
		        new Vector(deltaX, 0, deltaZ).scale_(BALL_EDGE_COEFF_FRICTION));
		ball.getAngularVelocity().rotate_(-edgeAngle);
	}

	/**
	 * Apply a force on a ball at a given position.
	 * Note that thid method assumes the universe is centered on the
	 * ball, the applied force is parallel to the y-axis and the point
	 * of contact is somewhere on the surface of the ball.
	 * @param b The ball on which to apply the force
	 * @param pointOfContact The point on the ball at which to apply the
	 * force
	 * @param force The force to apply
	 */
	public static void applyForce(Ball b, Vector pointOfContact,
	                              Vector force)
	{
		/* Force represents the endpoint of the vector */
		assert(Math.abs(force.getElements()[0]) < 0.0001);
		assert(Math.abs(force.getElements()[2]) < 0.0001);
		if (force.magnitude() == 0)
			return;

		double F = force.magnitude();
		double[] coords = pointOfContact.getElements();
		b.getVelocity().add_(new Vector(0, F, 0));
		b.getAngularVelocity().add_(new Vector(coords[2] * -F,
		                                       0,
		                                       coords[0] * -F));
		double inertia = 5 / (2 * b.getRadius() * b.getRadius());
		b.getAngularVelocity().scale_(inertia);
		/* For explanation of the six lines above, see physimI.pdf,
		 * second and third page */
	}

	/**
	 * Prepare the necessare vectors for the cue shot.
	 * This rotates the universe so the force of the cue is exactly
	 * parallel to the y-axis, and shifts the contents of the universe
	 * so the center of the ball is at the center of the universe.
	 *
	 * @param force The force vector to be prepared
	 * @param pointOfContact The vector representing the point of contact
	 * to be prepared
	 * @param b The ball to be prepared
	 * @param XYangle The angle of the cue force relative to the y-axis
	 * @param Zangle The angle of the cue force relative to the x-y plane
	 * @return The position of the ball in the old reference frame, which
	 * should be passed to restoreFromCueShot to return the universe
	 * to normal
	 */
	private static Vector prepareCueShot(Vector force,
	                                     Vector pointOfContact,
	                                     Ball b,
	                                     final double XYangle,
	                                     final double Zangle)
	{
		Vector oldFrame = b.getPosition().clone();
		/* Center the universe on the point of contact */
		Vector ballPosition = b.getPosition();
		Vector ballVelocity = b.getVelocity();
		Vector ballAngularVelocity = b.getAngularVelocity();
		ballPosition.sub_(pointOfContact);
		pointOfContact.sub_(pointOfContact);

		/* Rotate the universe so the cue is exactly parallel to the
		 * y-axis */
		force.rotate_(-XYangle);
		ballPosition.rotate_(-XYangle);
		ballVelocity.rotate_(-XYangle);
		ballAngularVelocity.rotate_(-XYangle);
		assert(Math.abs(force.getElements()[0]) < 0.00001);

		force.rotateAroundX_(-Zangle);
		ballPosition.rotateAroundX_(-Zangle);
		ballVelocity.rotateAroundX_(-Zangle);
		ballAngularVelocity.rotateAroundX_(-Zangle);
		assert(Math.abs(force.getElements()[2]) < 0.00001);

		/* Center the universe on the center of the ball */
		pointOfContact.sub_(ballPosition);
		ballPosition.sub_(ballPosition);

		b.setPosition(ballPosition);
		b.setVelocity(ballVelocity);
		b.setAngularVelocity(ballAngularVelocity);
		return oldFrame;
	}

	/**
	 * Restores the universe to its natural position after the cue shot.
	 * Note that this method only sets the state of the ball correctly. The
	 * pointOfContact is needed but not altered in any way.
	 * @param b The ball of which to restore the state
	 * @param XYangle The original angle of the applied force relative
	 * to the x-axis
	 * @param Zangle The original angle of the applied force relative
	 * to the x-y plane
	 * @param pointOfContact The point at which the force was applied.
	 * @param oldFrame The old position of the ball, as returned by
	 * prepareCueShot
	 */
	private static void restoreFromCueShot(Ball b,
	                                       final double XYangle,
	                                       final double Zangle,
	                                       Vector pointOfContact,
	                                       Vector oldFrame)
	{
		Vector ballPosition = b.getPosition();
		Vector ballVelocity = b.getVelocity();
		Vector ballAngularVelocity = b.getAngularVelocity();

		/* First we center the universe on the point of contact */
		ballPosition.sub_(pointOfContact);

		/* Then we rotate everything back */
		ballPosition.rotateAroundX_(Zangle);
		ballVelocity.rotateAroundX_(Zangle);
		ballAngularVelocity.rotateAroundX_(Zangle);

		ballPosition.rotate_(XYangle);
		ballVelocity.rotate_(XYangle);
		ballAngularVelocity.rotate_(XYangle);

		/* And then we go back to the table-centric frame of
		 * reference */
		b.setPosition(oldFrame);
	}

	/**
		Calculates the velocity and Angle of the shot (made as input) on
		a ball
	*/
	public static void cueShot(Cue c, Ball b, boolean includeNoise)
	{
		Vector force = c.getForce().clone();
		Vector pointOfContact = c.getPosition().clone();
		force.scale_(4);
		if (force.magnitude() > 500)
			force.scaleTo_(500);
		if (includeNoise) {
			double forceMagnitude = force.magnitude();
			double noiseStdDev = Game.getSettings().getForceNoise();
			forceMagnitude =
				Util.getRandomBoundedNormal(forceMagnitude,
				                            noiseStdDev,
			                                    0,
							    510);
			force.scaleTo_(forceMagnitude);
			noiseStdDev = Game.getSettings().getAngleNoise();
			double angle = Util.getRandomBoundedNormal(0, noiseStdDev, -Math.PI, Math.PI);
			force = force.rotate_(angle);
		}

		final double XYangle = Vector.angle(force, new Vector(0, -1,0));
		final double Zangle = -force.Zangle();
		synchronized (b) {
			Vector oldFrame;
			oldFrame = prepareCueShot(force, pointOfContact, b,
			                          XYangle, Zangle);
			if (includeNoise) {
				double angle = Util.getRandomBoundedNormal(0, Math.PI / 1440, -Math.PI, Math.PI);
				double noiseStdDev = Game.getSettings().getPoCNoise();
				pointOfContact = pointOfContact.rotate_(angle);
				angle = Util.getRandomBoundedNormal(0, noiseStdDev, -Math.PI, Math.PI);
				pointOfContact = pointOfContact.rotateAroundX_(angle);
			}
			double[] coords = pointOfContact.getElements();
			double inertia = 5 / (2*b.getRadius()*b.getRadius());
			inertia *= (coords[0]*coords[0] + coords[2]*coords[2]);
			force.scale_(1 / (1 + CUE_MASS_RATIO + inertia));
			applyForce(b, pointOfContact, force);
			restoreFromCueShot(b, XYangle, Zangle, pointOfContact,
			                   oldFrame);
		}
		b.getVelocity().mul_(new Vector(1, 1, 0)); /* We shouldn't have any Z-velocity */
	}

	public static boolean isBallInPocket(Ball b, Ellipse2D p)
	{
		return p.contains(b.getPosition().getElements()[0],
		                  b.getPosition().getElements()[1]);
	}
}
